#include "det/postprocess.h"

#include <iostream>
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>

#include <set>
#include <vector>

// 更新为您的15种物资类别标签
static const char *labels[OBJ_CLASS_NUM] = {
    "物资1", "物资2", "物资3", "物资4", "物资5",
    "物资6", "物资7", "物资8", "物资9", "物资10",
    "物资11", "物资12", "物资13", "物资14", "物资15"
};

// YOLOv5 anchors (保留用于兼容)
const int anchor0[6] = {10, 13, 16, 30, 33, 23};
const int anchor1[6] = {30, 61, 62, 45, 59, 119};
const int anchor2[6] = {116, 90, 156, 198, 373, 326};

static const int cnum = 2;
static cv::Scalar_<int> randColor[cnum];
static bool init_colors = false;

// 初始化随机颜色
static void initializeRandColors()
{
    if (!init_colors)
    {
        randColor[0] = cv::Scalar(0, 255, 0); // 绿色
        randColor[1] = cv::Scalar(0, 0, 255); // 红色
        init_colors = true;
    }
}

// 限制数值在指定范围内
inline static int clamp(float val, int min, int max)
{
    return val > min ? (val < max ? val : max) : min;
}

// 计算两个边界框的交并比
// 用于非极大值抑制(NMS) 算法中判断重叠程度

static float CalculateOverlap(float xmin0, float ymin0, float xmax0, float ymax0, float xmin1, float ymin1, float xmax1,
                              float ymax1)
{
    float w = fmax(0.f, fmin(xmax0, xmax1) - fmax(xmin0, xmin1) + 1.0);
    float h = fmax(0.f, fmin(ymax0, ymax1) - fmax(ymin0, ymin1) + 1.0);
    float i = w * h;
    float u = (xmax0 - xmin0 + 1.0) * (ymax0 - ymin0 + 1.0) + (xmax1 - xmin1 + 1.0) * (ymax1 - ymin1 + 1.0) - i;
    return u <= 0.f ? 0.f : (i / u);
}

// 非极大值抑制 实现非极大值抑制算法 用于过滤重复的检测框 通过IoU阈值去除重叠的边界框
static int nms(int validCount, std::vector<float> &outputLocations, std::vector<int> classIds, std::vector<int> &order,
               int filterId, float threshold)
{
    for (int i = 0; i < validCount; ++i)
    {
        if (order[i] == -1 || classIds[i] != filterId)
        {
            continue;
        }
        int n = order[i];
        for (int j = i + 1; j < validCount; ++j)
        {
            int m = order[j];
            if (m == -1 || classIds[i] != filterId)
            {
                continue;
            }
            float xmin0 = outputLocations[n * 4 + 0];
            float ymin0 = outputLocations[n * 4 + 1];
            float xmax0 = outputLocations[n * 4 + 0] + outputLocations[n * 4 + 2];
            float ymax0 = outputLocations[n * 4 + 1] + outputLocations[n * 4 + 3];

            float xmin1 = outputLocations[m * 4 + 0];
            float ymin1 = outputLocations[m * 4 + 1];
            float xmax1 = outputLocations[m * 4 + 0] + outputLocations[m * 4 + 2];
            float ymax1 = outputLocations[m * 4 + 1] + outputLocations[m * 4 + 3];

            float iou = CalculateOverlap(xmin0, ymin0, xmax0, ymax0, xmin1, ymin1, xmax1, ymax1);

            if (iou > threshold)
            {
                order[j] = -1;
            }
        }
    }
    return 0;
}

// 逆序快速排序，按置信度排序检测结果
static int quick_sort_indice_inverse(std::vector<float> &input, int left, int right, std::vector<int> &indices)
{
    float key;
    int key_index;
    int low = left;
    int high = right;
    if (left < right)
    {
        key_index = indices[left];
        key = input[left];
        while (low < high)
        {
            while (low < high && input[high] <= key)
            {
                high--;
            }
            input[low] = input[high];
            indices[low] = indices[high];
            while (low < high && input[low] >= key)
            {
                low++;
            }
            input[high] = input[low];
            indices[high] = indices[low];
        }
        input[low] = key;
        indices[low] = key_index;
        quick_sort_indice_inverse(input, left, low - 1, indices);
        quick_sort_indice_inverse(input, low + 1, right, indices);
    }
    return low;
}

// 量化相关 实现浮点数与定点数之间的量化和反量化操作 用于模型推理中的数值精度转换 支持INT8量化模型的处理
inline static int32_t __clip(float val, float min, float max)
{
    float f = val <= min ? min : (val >= max ? max : val);
    return f;
}
// 模型推理前将浮点输入数据转换为INT8格式
static int8_t qnt_f32_to_affine(float f32, int32_t zp, float scale)
{
    float dst_val = (f32 / scale) + zp;
    int8_t res = (int8_t)__clip(dst_val, -128, 127);
    return res;
}
// 模型推理后将INT8输出转换回浮点数以便后续处理
static float deqnt_affine_to_f32(int8_t qnt, int32_t zp, float scale)
{
    return ((float)qnt - (float)zp) * scale;
}

// 实用函数：将RKNN FP16输出转换为浮点数
static float fp16_to_fp32(uint16_t fp16_val) {
    // 按IEEE-754标准从FP16转换到FP32
    uint32_t sign = (fp16_val >> 15) & 0x1;
    uint32_t exponent = (fp16_val >> 10) & 0x1F;
    uint32_t fraction = fp16_val & 0x3FF;
    
    uint32_t fp32_val;
    
    if (exponent == 0) {
        if (fraction == 0) {
            // 零值
            fp32_val = sign << 31;
        } else {
            // 非规格化数值
            int e = -14;
            while ((fraction & 0x400) == 0) {
                fraction <<= 1;
                e--;
            }
            fraction &= 0x3FF;
            exponent = e + 127;
            fp32_val = (sign << 31) | (exponent << 23) | (fraction << 13);
        }
    } else if (exponent == 0x1F) {
        // 无穷大或NaN
        fp32_val = (sign << 31) | (0xFF << 23) | (fraction << 13);
    } else {
        // 规格化数值
        exponent = exponent + (127 - 15);
        fp32_val = (sign << 31) | (exponent << 23) | (fraction << 13);
    }
    
    float result;
    memcpy(&result, &fp32_val, sizeof(result));
    return result;
}

static int process(int8_t *input, int *anchor, int grid_h, int grid_w, int height, int width, int stride,
                   std::vector<float> &boxes, std::vector<float> &objProbs, std::vector<int> &classId, float threshold,
                   int32_t zp, float scale)
{
    int validCount = 0;
    int grid_len = grid_h * grid_w;
    int8_t thres_i8 = qnt_f32_to_affine(threshold, zp, scale);
    for (int a = 0; a < 3; a++)
    {
        for (int i = 0; i < grid_h; i++)
        {
            for (int j = 0; j < grid_w; j++)
            {
                int8_t box_confidence = input[(PROP_BOX_SIZE * a + 4) * grid_len + i * grid_w + j];
                if (box_confidence >= thres_i8)
                {
                    int offset = (PROP_BOX_SIZE * a) * grid_len + i * grid_w + j;
                    int8_t *in_ptr = input + offset;
                    float box_x = (deqnt_affine_to_f32(*in_ptr, zp, scale)) * 2.0 - 0.5;
                    float box_y = (deqnt_affine_to_f32(in_ptr[grid_len], zp, scale)) * 2.0 - 0.5;
                    float box_w = (deqnt_affine_to_f32(in_ptr[2 * grid_len], zp, scale)) * 2.0;
                    float box_h = (deqnt_affine_to_f32(in_ptr[3 * grid_len], zp, scale)) * 2.0;
                    box_x = (box_x + j) * (float)stride;
                    box_y = (box_y + i) * (float)stride;
                    box_w = box_w * box_w * (float)anchor[a * 2];
                    box_h = box_h * box_h * (float)anchor[a * 2 + 1];
                    box_x -= (box_w / 2.0);
                    box_y -= (box_h / 2.0);

                    int8_t maxClassProbs = in_ptr[5 * grid_len];
                    int maxClassId = 0;
                    for (int k = 1; k < OBJ_CLASS_NUM; ++k)
                    {
                        int8_t prob = in_ptr[(5 + k) * grid_len];
                        if (prob > maxClassProbs)
                        {
                            maxClassId = k;
                            maxClassProbs = prob;
                        }
                    }
                    if (maxClassProbs > thres_i8)
                    {
                        objProbs.push_back((deqnt_affine_to_f32(maxClassProbs, zp, scale)) *
                                           (deqnt_affine_to_f32(box_confidence, zp, scale)));
                        classId.push_back(maxClassId);
                        validCount++;
                        boxes.push_back(box_x);
                        boxes.push_back(box_y);
                        boxes.push_back(box_w);
                        boxes.push_back(box_h);
                    }
                }
            }
        }
    }
    return validCount;
}

// YOLOv8处理函数 - 无anchor boxes
static int process_yolov8(int8_t *input, int grid_h, int grid_w, int height, int width, std::vector<float> &boxes,
                          std::vector<float> &objProbs, std::vector<int> &classId, float threshold, int32_t zp,
                          float scale)
{
    int validCount = 0;
    // int grid_len = grid_h * grid_w; // 未使用的变量
    int8_t *grid_ptr = input;

    for (int h = 0; h < grid_h; h++)
    {
        for (int w_idx = 0; w_idx < grid_w; w_idx++)
        {
            int grid_index = h * grid_w + w_idx;
            int8_t *row_ptr = grid_ptr + grid_index * PROP_BOX_SIZE;

            // YOLOv8输出格式: [x, y, w, h, class_scores...]
            float x = deqnt_affine_to_f32(row_ptr[0], zp, scale);
            float y = deqnt_affine_to_f32(row_ptr[1], zp, scale);
            float w = deqnt_affine_to_f32(row_ptr[2], zp, scale);
            float h = deqnt_affine_to_f32(row_ptr[3], zp, scale);

            // 找到最大类别置信度
            float max_class_score = 0;
            int max_class_id = 0;

            for (int c = 0; c < OBJ_CLASS_NUM; c++)
            {
                float class_score = deqnt_affine_to_f32(row_ptr[4 + c], zp, scale);
                if (class_score > max_class_score)
                {
                    max_class_score = class_score;
                    max_class_id = c;
                }
            }

            // 检查置信度阈值
            if (max_class_score > threshold)
            {
                // YOLOv8输出是归一化的，直接转换为像素坐标
                float x_center = x * width;
                float y_center = y * height;
                float box_w = w * width;
                float box_h = h * height;

                // 转换为左上角坐标
                float x1 = x_center - box_w / 2.0f;
                float y1 = y_center - box_h / 2.0f;

                // 确保坐标在有效范围内
                x1 = clamp(x1, 0, width - 1);
                y1 = clamp(y1, 0, height - 1);
                box_w = clamp(box_w, 1, width - x1);
                box_h = clamp(box_h, 1, height - y1);

                boxes.push_back(x1);
                boxes.push_back(y1);
                boxes.push_back(box_w);
                boxes.push_back(box_h);
                objProbs.push_back(max_class_score);
                classId.push_back(max_class_id);
                validCount++;
            }
        }
    }

    return validCount;
}

// int post_process(int8_t *input0, int8_t *input1, int8_t *input2, int model_in_h, int model_in_w, float conf_threshold,
//                  float nms_threshold, BOX_RECT pads, float scale_w, float scale_h, std::vector<int32_t> &qnt_zps,
//                  std::vector<float> &qnt_scales, DetectionResultsGroup *group)
// {
//     // 安全地初始化结构体
//     group->cur_img = cv::Mat();
//     group->cur_frame_id = 0;
//     group->fps = 0.0f;
//     group->dets.clear();

//     std::vector<float> filterBoxes;
//     std::vector<float> objProbs;
//     std::vector<int> classId;

//     // stride 8
//     int stride0 = 8;
//     int grid_h0 = model_in_h / stride0;
//     int grid_w0 = model_in_w / stride0;
//     int validCount0 = 0;
//     validCount0 = process(input0, (int *)anchor0, grid_h0, grid_w0, model_in_h, model_in_w, stride0, filterBoxes,
//                           objProbs, classId, conf_threshold, qnt_zps[0], qnt_scales[0]);

//     // stride 16
//     int stride1 = 16;
//     int grid_h1 = model_in_h / stride1;
//     int grid_w1 = model_in_w / stride1;
//     int validCount1 = 0;
//     validCount1 = process(input1, (int *)anchor1, grid_h1, grid_w1, model_in_h, model_in_w, stride1, filterBoxes,
//                           objProbs, classId, conf_threshold, qnt_zps[1], qnt_scales[1]);

//     // stride 32
//     int stride2 = 32;
//     int grid_h2 = model_in_h / stride2;
//     int grid_w2 = model_in_w / stride2;
//     int validCount2 = 0;
//     validCount2 = process(input2, (int *)anchor2, grid_h2, grid_w2, model_in_h, model_in_w, stride2, filterBoxes,
//                           objProbs, classId, conf_threshold, qnt_zps[2], qnt_scales[2]);

//     int validCount = validCount0 + validCount1 + validCount2;
//     // no object detect
//     if (validCount <= 0)
//     {
//         return 0;
//     }

//     std::vector<int> indexArray;
//     for (int i = 0; i < validCount; ++i)
//     {
//         indexArray.push_back(i);
//     }

//     quick_sort_indice_inverse(objProbs, 0, validCount - 1, indexArray);

//     std::set<int> class_set(std::begin(classId), std::end(classId));

//     for (auto c : class_set)
//     {
//         nms(validCount, filterBoxes, classId, indexArray, c, nms_threshold);
//     }

//     /* box valid detect target */
//     for (int i = 0; i < validCount; ++i)
//     {
//         // if (indexArray[i] == -1 || last_count >= OBJ_NUMB_MAX_SIZE)
//         if (indexArray[i] == -1)
//         {
//             continue;
//         }
//         int n = indexArray[i];

//         float x1 = filterBoxes[n * 4 + 0] - pads.left;
//         float y1 = filterBoxes[n * 4 + 1] - pads.top;
//         float x2 = x1 + filterBoxes[n * 4 + 2];
//         float y2 = y1 + filterBoxes[n * 4 + 3];
//         int id = classId[n];
//         // float obj_conf = objProbs[i]; // 未使用的变量

//         DetectionBox new_box;
//         int _x1 = (int)(clamp(x1, 0, model_in_w) / scale_w);
//         int _y1 = (int)(clamp(y1, 0, model_in_h) / scale_h);
//         int _x2 = (int)(clamp(x2, 0, model_in_w) / scale_w);
//         int _y2 = (int)(clamp(y2, 0, model_in_h) / scale_h);
//         new_box.box = cv::Rect_<int>(_x1, _y1, _x2 - _x1, _y2 - _y1);
//         new_box.score = objProbs[i];
//         new_box.det_name = labels[id];
//         group->dets.push_back(new_box);
//     }

//     return 0;
// }

int draw_image_detect(cv::Mat &cur_img, std::vector<DetectionBox> &results, int cur_frame_id)
{
    char text[256];
    for (const auto &res : results)
    {
        sprintf(text, "%s", res.det_name.c_str());
        cv::rectangle(cur_img, res.box, cv::Scalar(256, 0, 0, 256), 3);
        cv::putText(cur_img, text, cv::Point(res.box.x, res.box.y + 12), cv::FONT_HERSHEY_SIMPLEX, 0.4,
                    cv::Scalar(255, 255, 255));
    }
    std::ostringstream oss;

    oss << "detect_" << std::setfill('0') << std::setw(4) << cur_frame_id << ".jpg";

    std::string filename = oss.str();

    if (!cv::imwrite(filename, cur_img))
    {
        return -1;
    }
    return 0;
}

void show_draw_results(DetectionResultsGroup &results_group)
{
    if (!init_colors)
    {
        initializeRandColors();
    }

    // 检查图像是否为空
    if (results_group.cur_img.empty())
    {
        return;
    }

    // 获取图像尺寸
    int img_width = results_group.cur_img.cols;
    int img_height = results_group.cur_img.rows;
    int center_x = img_width / 2;
    int center_y = img_height / 2;

    // 在图像中心画十字
    int cross_size = 20;                 // 十字大小
    cv::Scalar cross_color(0, 255, 255); // 青色十字
    int cross_thickness = 2;

    // 画水平线
    cv::line(results_group.cur_img, cv::Point(center_x - cross_size, center_y),
             cv::Point(center_x + cross_size, center_y), cross_color, cross_thickness, cv::LINE_AA);

    // 画垂直线
    cv::line(results_group.cur_img, cv::Point(center_x, center_y - cross_size),
             cv::Point(center_x, center_y + cross_size), cross_color, cross_thickness, cv::LINE_AA);

    // 在图像左上角显示帧率信息
    char fps_text[64];
    sprintf(fps_text, "FPS: %.1f", results_group.fps);
    cv::putText(results_group.cur_img, fps_text, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0,
                cv::Scalar(80, 255, 255), 1, cv::LINE_AA);

    // 显示帧数信息
    char frame_text[64];
    // sprintf(frame_text, "Frame: %d", results_group.cur_frame_id);
    // cv::putText(results_group.cur_img, frame_text, cv::Point(10, 70), cv::FONT_HERSHEY_SIMPLEX, 0.8,
    //             cv::Scalar(0, 255, 255), 2, cv::LINE_AA);

    char text[256];
    for (const auto &res : results_group.dets)
    {
        sprintf(text, "%s %.2f", res.det_name.c_str(), res.score);
        cv::rectangle(results_group.cur_img, res.box, randColor[1], 2, 8, 0);
        cv::putText(results_group.cur_img, text, cv::Point(res.box.x, res.box.y + 12), cv::FONT_HERSHEY_SIMPLEX, 0.4,
                    cv::Scalar(255, 255, 255));

        int cv_center_x = res.box.x + res.box.width / 2;
        int cv_center_y = res.box.y + res.box.height / 2;

        cv::circle(results_group.cur_img, cv::Point(cv_center_x, cv_center_y), 10, cv::Scalar(0, 0, 255), -1,
                   cv::LINE_AA);
    }

    // 如果有检测到目标，画黄色预测点
    // if (!results_group.dets.empty())
    // {
    //     // 使用第一个检测结果作为预测点（如果有多个目标，可以根据需要调整逻辑）
    //     const auto &res = results_group.dets[0];
    //     int target_center_x = res.box.x + res.box.width / 2;
    //     int target_center_y = res.box.y + res.box.height / 2;

    //     // 画黄色预测点
    //     cv::Scalar yellow_color(0, 255, 130); // BGR格式，黄色
    //     int point_radius = 5;
    //     cv::circle(results_group.cur_img, cv::Point(target_center_x, target_center_y), point_radius, yellow_color,
    //     -1,
    //                cv::LINE_AA);
    // }
}
// YOLOv8后处理函数 - 单输出层
int post_process_yolov8(uint16_t *input, int model_in_h, int model_in_w, float conf_threshold, float nms_threshold,
    BOX_RECT pads, float scale_w, float scale_h, std::vector<int32_t> &qnt_zps,
    std::vector<float> &qnt_scales, DetectionResultsGroup *group,
    int num_outputs, uint32_t* output_dims)
{
    // 安全地初始化结构体
    group->cur_img = cv::Mat();
    group->cur_frame_id = 0;
    group->fps = 0.0f;
    group->dets.clear();

    std::vector<float> filterBoxes;
    std::vector<float> objProbs;
    std::vector<int> classId;

    // 【关键修复】根据实际模型输出维度确定检测框数量和大小
    int num_boxes = 8400; // 默认值，从模型输出信息得到
    int box_size = 19;    // 默认值

    // 使用实际的输出维度信息
    if (num_outputs > 0 && output_dims != nullptr) {
        if (num_outputs >= 3) {
            num_boxes = output_dims[2];  // 第3个维度是框数量：8400
            if (num_outputs >= 2) {
                box_size = output_dims[1]; // 第2个维度是每框属性：19
            }
        }
    }

    printf("=== YOLOv8后处理（FP16格式）===\n");
    printf("处理模型输出，框数量=%d，每框属性=%d\n", num_boxes, box_size);
    printf("模型输入尺寸: %dx%d, 缩放因子: scale_w=%.3f, scale_h=%.3f\n", 
           model_in_w, model_in_h, scale_w, scale_h);

    // 检查输出数据大小是否合理
    if (input == nullptr)
    {
        std::cerr << "YOLOv8输出数据为空" << std::endl;
        return -1;
    }

    // YOLOv8输出格式为[1, 19, 8400]，FP16格式
    uint16_t *output_fp16 = (uint16_t *)input;
    
    // 【关键修复】正确解析模型输出格式
    printf("\n=== 模型输出解析 ===\n");
    printf("输出维度: [1, %d, %d]\n", box_size, num_boxes);
    printf("预期格式: [x_center, y_center, width, height] + 15个类别分数\n");
    
    // 【安全检查】限制检查的框数量防止越界
    int check_boxes = std::min(3, num_boxes);
    printf("\n=== 前%d个检测框的原始数据检查 ===\n", check_boxes);
    for (int i = 0; i < check_boxes; i++) {
        printf("框 %d:\n", i);
        
        // 【边界检查】确保不越界访问
        if (i >= num_boxes) break;
        
        // 坐标数据 (4个值)
        float x = fp16_to_fp32(output_fp16[0 * num_boxes + i]);
        float y = fp16_to_fp32(output_fp16[1 * num_boxes + i]);
        float w = fp16_to_fp32(output_fp16[2 * num_boxes + i]);
        float h = fp16_to_fp32(output_fp16[3 * num_boxes + i]);
        
        printf("  坐标: x=%.4f, y=%.4f, w=%.4f, h=%.4f\n", x, y, w, h);
        
        // 类别分数 (15个类别) - 添加边界检查
        printf("  前8个类别分数: ");
        for (int c = 0; c < 8 && c < OBJ_CLASS_NUM && (4 + c) < box_size; c++) {
            float score = fp16_to_fp32(output_fp16[(4 + c) * num_boxes + i]);
            printf("%.4f ", score);
        }
        printf("\n");
        
        // 找最大分数 - 添加边界检查
        float max_score = 0;
        int max_class = 0;
        for (int c = 0; c < OBJ_CLASS_NUM && (4 + c) < box_size; c++) {
            float score = fp16_to_fp32(output_fp16[(4 + c) * num_boxes + i]);
            if (score > max_score) {
                max_score = score;
                max_class = c;
            }
        }
        printf("  最大类别分数: %.4f (类别%d)\n", max_score, max_class);
    }
    
    // 【修复】为15类物资检测优化阈值策略
    float real_threshold = conf_threshold;
    
    // 【安全检查】限制统计样本数量
    float max_found_score = 0;
    int stats_boxes_checked = std::min(1000, num_boxes);
    
    for (int i = 0; i < stats_boxes_checked; i++) {
        for (int c = 0; c < OBJ_CLASS_NUM && (4 + c) < box_size; c++) {
            float score = fp16_to_fp32(output_fp16[(4 + c) * num_boxes + i]);
            if (score > max_found_score) {
                max_found_score = score;
            }
        }
    }
    
    printf("检查前%d个框，找到最大置信度: %.6f\n", stats_boxes_checked, max_found_score);
    
    // 如果找到的最大分数很低，可能需要调整阈值
    if (max_found_score < real_threshold && max_found_score > 0.001f) {
        real_threshold = max_found_score * 0.5f;
        printf("调整置信度阈值: %.6f -> %.6f\n", conf_threshold, real_threshold);
    }
    
    printf("使用置信度阈值: %.6f\n", real_threshold);

    int validCount = 0;
    // 【关键修复】限制处理的框数量，防止内存问题和长时间处理
    int max_process = std::min(2000, num_boxes); // 减少到2000个框

    printf("开始处理前%d个检测框...\n", max_process);
    
    // 【诊断】统计置信度分布，帮助调试 - 限制样本数量
    std::vector<float> confidence_samples;
    int sample_count = std::min(200, num_boxes);
    
    for (int i = 0; i < sample_count; i++) {
        float max_score = 0;
        for (int c = 0; c < OBJ_CLASS_NUM && (4 + c) < box_size; c++) {
            float score = fp16_to_fp32(output_fp16[(4 + c) * num_boxes + i]);
            if (score > max_score) max_score = score;
        }
        confidence_samples.push_back(max_score);
    }
    
    // 排序以找到分位数
    std::sort(confidence_samples.begin(), confidence_samples.end(), std::greater<float>());
    
    printf("置信度分布分析（前%d个框）:\n", sample_count);
    if (confidence_samples.size() > 0) {
        printf("  最高: %.6f, 第10位: %.6f, 第50位: %.6f\n", 
               confidence_samples[0], 
               confidence_samples[std::min(9, (int)confidence_samples.size()-1)],
               confidence_samples[std::min(49, (int)confidence_samples.size()-1)]);
    }

    // 【自适应阈值】如果模型输出的置信度普遍偏低，自动调整阈值
    float adaptive_threshold = real_threshold;
    if (confidence_samples.size() > 0 && confidence_samples[0] < 0.3f && confidence_samples[0] > 0.001f) {
        adaptive_threshold = confidence_samples[std::min(19, (int)confidence_samples.size()-1)];
        printf("⚠️  检测到置信度偏低，自适应调整阈值: %.6f -> %.6f\n", real_threshold, adaptive_threshold);
    }

    // 【修复】使用YOLOv8标准转置布局解析数据
    printf("使用YOLOv8标准转置布局解析输出数据\n");
    
    for (int i = 0; i < max_process && i < num_boxes; i++)
    {
        // 【边界检查】确保索引不越界
        if (i >= num_boxes) break;
        
        // 转置布局：每个属性的所有框连续存储（YOLOv8标准格式）
        float x = fp16_to_fp32(output_fp16[0 * num_boxes + i]);  // x_center
        float y = fp16_to_fp32(output_fp16[1 * num_boxes + i]);  // y_center
        float w = fp16_to_fp32(output_fp16[2 * num_boxes + i]);  // width
        float h = fp16_to_fp32(output_fp16[3 * num_boxes + i]);  // height
        
        // 查找最大类别置信度（15个类别）
        float max_class_score = 0;
        int max_class_id = 0;
        
        // 【关键修复】添加边界检查防止越界访问
        for (int c = 0; c < OBJ_CLASS_NUM && (4 + c) < box_size; c++)
        {
            float class_score = fp16_to_fp32(output_fp16[(4 + c) * num_boxes + i]);
            if (class_score > max_class_score)
            {
                max_class_score = class_score;
                max_class_id = c;
            }
        }
        
        // 对前5个框进行详细调试输出
        if (i < 5) {
            printf("框 %d: x=%.4f, y=%.4f, w=%.4f, h=%.4f\n", i, x, y, w, h);
            printf("  前8个类别分数: ");
            for (int c = 0; c < std::min(OBJ_CLASS_NUM, 8) && (4 + c) < box_size; c++) {
                float score = fp16_to_fp32(output_fp16[(4 + c) * num_boxes + i]);
                printf("c%d:%.4f ", c, score);
            }
            printf("\n  最大置信度=%.4f, 类别=%d, 阈值=%.4f\n", max_class_score, max_class_id, real_threshold);
        }
        
        // 使用自适应阈值过滤
        if (max_class_score > adaptive_threshold)
        {
            printf("框 %d 通过阈值检测！置信度=%.3f\n", i, max_class_score);
            
            // YOLOv8输出格式：中心点坐标和宽高，都是归一化的
            float x_center = x;
            float y_center = y;
            float box_w = w;
            float box_h = h;
            
            // 转换为像素坐标
            float final_x1 = (x_center - box_w / 2.0f) * model_in_w;
            float final_y1 = (y_center - box_h / 2.0f) * model_in_h;
            float final_width = box_w * model_in_w;
            float final_height = box_h * model_in_h;
            
            printf("  归一化坐标: 中心(%.3f,%.3f), 尺寸(%.3f,%.3f)\n", x_center, y_center, box_w, box_h);
            printf("  像素坐标: x1=%.1f, y1=%.1f, w=%.1f, h=%.1f\n", 
                   final_x1, final_y1, final_width, final_height);

            // 确保最终坐标在有效范围内
            final_x1 = std::max(0.0f, final_x1);
            final_y1 = std::max(0.0f, final_y1);
            
            float final_x2 = std::min(final_x1 + final_width, (float)model_in_w);
            float final_y2 = std::min(final_y1 + final_height, (float)model_in_h);
            
            final_width = final_x2 - final_x1;
            final_height = final_y2 - final_y1;

            // 检查框的合理性
            if (final_width > 8 && final_height > 8 && 
                final_width < model_in_w * 0.95 && final_height < model_in_h * 0.95)
            {
                filterBoxes.push_back(final_x1);
                filterBoxes.push_back(final_y1);
                filterBoxes.push_back(final_width);
                filterBoxes.push_back(final_height);
                objProbs.push_back(max_class_score);
                classId.push_back(max_class_id);
                validCount++;
                
                printf("✓ 添加有效框 %d: x1=%.1f, y1=%.1f, w=%.1f, h=%.1f, 置信度=%.3f, 类别=%d\n", 
                       validCount, final_x1, final_y1, final_width, final_height, max_class_score, max_class_id);
            } else {
                printf("✗ 框不合理，忽略: x1=%.1f, y1=%.1f, w=%.1f, h=%.1f\n", 
                       final_x1, final_y1, final_width, final_height);
            }
        }
    }

    printf("最终检测到 %d 个有效框\n", validCount);

    // 没有检测到对象
    if (validCount <= 0)
    {
        printf("没有检测到任何有效对象\n");
        printf("建议检查：\n");
        printf("1. 置信度阈值是否过高（当前%.3f）\n", real_threshold);
        printf("2. 模型输入图像是否正确\n");
        printf("3. 模型是否适配当前场景\n");
        return 0;
    }

    // 进一步限制结果数量
    if (validCount > 10) {
        printf("检测框过多，只保留前10个最高置信度的框\n");
        validCount = 10;
    }

    printf("=== 处理 %d 个有效检测框 ===\n", validCount);

    std::vector<int> indexArray;
    for (int i = 0; i < validCount; ++i)
    {
        indexArray.push_back(i);
    }

    // 按置信度排序
    quick_sort_indice_inverse(objProbs, 0, validCount - 1, indexArray);

    // 对每个类别进行NMS
    std::set<int> class_set(std::begin(classId), std::end(classId));
    for (auto c : class_set)
    {
        nms(validCount, filterBoxes, classId, indexArray, c, nms_threshold);
    }

    // 生成最终检测结果
    for (int i = 0; i < validCount; ++i)
    {
        if (indexArray[i] == -1)
        {
            continue;
        }
        int n = indexArray[i];

        // 获取边界框坐标和尺寸
        float x1 = filterBoxes[n * 4 + 0];
        float y1 = filterBoxes[n * 4 + 1];
        float box_w = filterBoxes[n * 4 + 2];
        float box_h = filterBoxes[n * 4 + 3];
        
        // 应用填充偏移
        x1 -= pads.left;
        y1 -= pads.top;
        
        float x2 = x1 + box_w;
        float y2 = y1 + box_h;
        int id = classId[n];

        // 【关键修复】添加边界检查防止数组越界
        if (id < 0 || id >= OBJ_CLASS_NUM) {
            printf("⚠️ 类别ID %d 超出范围，跳过该检测框\n", id);
            continue;
        }

        printf("最终框 %d: x=%.1f, y=%.1f, 宽=%.1f, 高=%.1f, 类别=%d (%s), 置信度=%.6f\n", 
               i+1, x1, y1, box_w, box_h, id, labels[id], objProbs[n]);

        // 应用缩放因子，转换回原始图像大小
        int _x1 = (int)(x1 / scale_w);
        int _y1 = (int)(y1 / scale_h);
        int _x2 = (int)(x2 / scale_w);
        int _y2 = (int)(y2 / scale_h);
        
        // 确保坐标在有效范围内
        _x1 = std::max(0, _x1);
        _y1 = std::max(0, _y1);
        _x2 = std::min(_x2, (int)(model_in_w / scale_w));
        _y2 = std::min(_y2, (int)(model_in_h / scale_h));
        
        // 计算宽高
        int width = _x2 - _x1;
        int height = _y2 - _y1;
        
        // 确保宽高为正
        if (width > 0 && height > 0)
        {
            DetectionBox new_box;
            new_box.box = cv::Rect_<int>(_x1, _y1, width, height);
            new_box.score = objProbs[n];
            new_box.det_name = labels[id];
            group->dets.push_back(new_box);
        }
    }

    printf("=== 后处理完成，返回 %d 个检测结果 ===\n", (int)group->dets.size());
    
    // 添加诊断建议
    if (group->dets.size() > 0) {
        printf("\n📋 检测结果分析:\n");
        printf("- 检测到 %d 个物体\n", (int)group->dets.size());
        printf("- 如果检测框位置固定不变，可能是坐标解析问题\n");
        printf("- 如果手挡住相机仍检测到物体，可能是模型过拟合或输入预处理问题\n");
        printf("- 建议检查模型训练数据和预处理流程\n");
    } else {
        printf("\n⚠️  未检测到任何物体，可能原因:\n");
        printf("- 置信度阈值过高\n");
        printf("- 模型与实际场景不匹配\n");
        printf("- 输入图像预处理不正确\n");
    }
    
    return 0;
}